#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, String
import numpy as np
import argparse
import rospkg
import os

def main():

    rospy.init_node('teach_move_commander', anonymous=True)
    pub = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)
    pub_traj = rospy.Publisher('/command_move_traj', String, queue_size=10)

    # 获取当前 ROS 包路径
    traj_path = "/home/zengyuhao/.ros/data_2025-06-25-00-08-24.txt"

    rospy.sleep(1)

    msg = Float64MultiArray()
    data = np.loadtxt(traj_path, delimiter=',')

    print("move to traj start point...")
    msg.data = data[0]
    pub.publish(msg)
    rospy.sleep(5.5)

    msg_traj = String()
    msg_traj.data = traj_path
    pub_traj.publish(msg_traj)

if __name__ == '__main__':
    main()
